/****************************************************************************
 * Elisa 1.0                                                                *
 *--------------------------------------------------------------------------*
 * Author: Laurent Granvilliers, Vincent Sorin                              *
 * Copyright (c) 2004 Laboratoire d'Informatique de Nantes Atlantique       *
 *--------------------------------------------------------------------------*
 * Elisa is distributed WITHOUT ANY WARRANTY. Read the associated           *
 * COPYRIGHT file for more details.                                         *
 *--------------------------------------------------------------------------*
 * elisa_newton_reduction.cpp                                               *
 ****************************************************************************/

#include "elisa_newton_reduction.h"


USING_ELISA


#ifdef ELISA_PROFILE
elisa::Clock elisa::NewtonReduction::ClockNewton;
long elisa::NewtonReduction::NumCall = 0;
#endif


//--------------------------------------------------------------------------
// class NewtonReduction
//--------------------------------------------------------------------------

// improvement factor = 10%
double NewtonReduction::StaticImprove = 0.9;


NewtonReduction::NewtonReduction(VarImplSet & vs,
                                 SetConstraints & s,
                                 IntervalFormFactory *createForm) :
	_MIzero(vs.size(), vs.size(), Interval(0.0)),
	_VIzero(vs.size(), 1, Interval(0.0)),
	_MRid(vs.size(), vs.size(), 0.0)
{
	// dimension of the system
	_dim = vs.size();


	// Each variable is added in the container of the reduction
	VarImplSet::iterator iv;
	for (iv = vs.begin(); iv != vs.end(); ++iv) {
		RealVarImpl *v = static_cast<RealVarImpl *>(iv->get());
		_vars.push_back(v);
	}


	// An interval form is associated to the functional expression
	// of each constraint
	SetConstraints::iterator it;
	for (it = s.begin(); it != s.end(); ++it) {
		post(*it);
		RealConstraint *c = static_cast<RealConstraint *>(*it);
		RealExpr *temp = c->function();
		_exprs.push_back(createForm->create(temp));
		delete temp;
	}

	// Creation of particular matrices
	for (int i = 1; i <= _dim; ++i) {
		_MRid(i, i) = 1.0;
	}

	// Improvement factor
	m_improve = StaticImprove;

	std::vector<RealVarImpl *>::iterator itVar;
	for (itVar = _vars.begin(); itVar != _vars.end(); ++itVar) {
		(*itVar)->dependsOn(this, DomainReduction);
	}
}


NewtonReduction::~NewtonReduction()
{
	// The interval forms are deleted
	std::vector<IntervalForm * >::iterator it;
	for (it = _exprs.begin(); it != _exprs.end(); ++it) {
		delete (*it);
	}
}


void NewtonReduction::evalMidpoint(DenseMatrix<Interval> & mp,
                                   DenseMatrix<Interval> & b) const
{
	std::vector<RealVarImpl *>::const_iterator it;

	// Saves all variable domains
	for (it = _vars.begin(); it != _vars.end(); ++it) {
		(*it)->saveDomain();
	}

	// Computes the interval midpoints and set the variable domains
	int i;
	for (i = 1, it = _vars.begin(); it != _vars.end(); ++it, ++i) {
		RealVarImpl *v = *it;
		Interval::Bound m = v->domain().mid();
		mp(i) = Interval(nextDown(m), nextUp(m));
		v->setQuietDomain(mp(i));
	}


	// Evaluation of -F(midpoint)
	std::vector<IntervalForm * >::const_iterator ie;
	for (i = 1, ie = _exprs.begin(); ie != _exprs.end(); ++ie, ++i) {
		b(i) = -(*ie)->eval();
	}


	// Restores all variable domains
	for (it = _vars.begin(); it != _vars.end(); ++it) {
		(*it)->restoreDomain();
	}
}


void NewtonReduction::evalVariable(DenseMatrix<Interval> & y,
                                   DenseMatrix<Interval> & mid) const
{
	std::vector<RealVarImpl *>::const_iterator it;
	int i;
	for (i = 1, it = _vars.begin(); it != _vars.end(); ++it, ++i) {
		y(i) = (*it)->domain() - mid(i);
	}
}


void NewtonReduction::evalJacobian(DenseMatrix<Interval> & m) const
{
	std::vector<IntervalForm * >::const_iterator ie;
	int i, j;
	for (i = 1, ie = _exprs.begin(); ie != _exprs.end(); ++ie, ++i) {
		std::vector<RealVarImpl *>::const_iterator it;
		for (j = 1, it = _vars.begin(); it != _vars.end(); ++it, ++j) {
			m(i, j) = (*ie)->deriv(**it);
		}
	}
}


void NewtonReduction::evalMidJacobian(DenseMatrix<double> & mid,
                                      DenseMatrix<Interval> & J) const
{
	for (int i = 1; i <= mid.nRows(); ++i) {
		for (int j = 1; j <= mid.nRows(); ++j) {
			mid(i, j) = toDouble(J(i, j).mid());
		}
	}
}


bool NewtonReduction::reduceDomains(DenseMatrix<Interval> & y,
                                    DenseMatrix<Interval> & mid) const
{
	std::vector<RealVarImpl *>::const_iterator it;
	int i;
	for (i = 1, it = _vars.begin(); it != _vars.end(); ++it, ++i) {
		// New domain
		Interval I = y(i) + mid(i);

		if (!I.intersect((*it)->domain()).isEmpty()) {
			// Is the domain improved enough?
			if (I.width() < m_improve * (*it)->domain().width()) {
				(*it)->setDomain(I);
			} else {
				(*it)->setQuietDomain(I);
			}
		} else {
			return false;
		}
	}
	return true;
}


bool NewtonReduction::Inversion(DenseMatrix<double> & p,
                                DenseMatrix<double> & m,
                                DenseMatrix<double> & id)
{
	// p := identity
	p = id;


	// Iteration for rows 1..n
	int k = 1;
	while (k <= p.nRows()) {
		// search for the maximal partial pivot
		double max = fabs(m(k, k));
		int l = k;
		for (int i = k + 1; i <= p.nRows(); ++i) { // i from row k+1 to row n
			double max2 = fabs(m(i, k));
			if (max < max2) {
				max = max2; l = i;
			}
		}


		// THE MATRIX CAN BE SINGULAR then false is returned
		if (max == 0.0) {
			return false;
		}

		if (l != k) { // inversion of the lines l and k in p and m
			for (int i = k; i <= p.nRows(); ++i) { // i from column k to column n
				double max = m(l, i);
				m(l, i) = m(k, i);
				m(k, i) = max;
			}
			for (int i = 1; i <= p.nRows(); ++i) {
				double max = p(l, i);
				p(l, i) = p(k, i);
				p(k, i) = max;
			}
		}


		// The pivot is now m(k,k)
		double pivot = m(k, k);


		// Division of the lines k in p and m by the pivot
		for (int i = k; i <= p.nRows(); ++i) {
			if (m(k, i) != 0.0) {
				m(k, i) /= pivot;
			}
		}
		for (int i = 1; i <= p.nRows(); ++i) {
			if (p(k, i) != 0.0) {
				p(k, i) /= pivot;
			}
		}

		for (int j = k + 1; j <= p.nRows(); ++j) { // lines under k
			double coeff = m(j, k);
			if (coeff != 0.0) {
				for (int i = k; i <= p.nRows(); ++i) {
					if (m(k, i) != 0.0) {
						m(j, i) -= coeff * m(k, i);
					}
				}
				for (int i = 1; i <= p.nRows(); ++i) {
					if (p(k, i) != 0.0) {
						p(j, i) -= coeff * p(k, i);
					}
				}
			}
		}
		++k;
	}

	for (int k = p.nRows(); k > 1; --k) {
		for (int j = k - 1; j >= 1; --j) {
			double coeff = m(j, k);
			m(j, k) = 0.0;
			if (coeff != 0.0) {
				for (int i = p.nRows(); i >= 1; --i) {
					if (p(k, i) != 0.0) {
						p(j, i) -= coeff * p(k, i);
					}
				}
			}
		}
	}
	return true;
}


bool NewtonReduction::GaussSeidel(DenseMatrix<Interval> & A,
                                  DenseMatrix<Interval> & x,
                                  DenseMatrix<Interval> & b)
{
	for (int i = 1; i <= A.nRows(); ++i) {
		// TO BE REMOVED WITH THE EXTENDED INTERVAL DIVISION
		if (A(i, i).overlaps(Interval::Zero)) {
			continue;
		}

		// Reduction of x(i)
		// I := b(i) - sum_j A(i,j)*x(j)
		Interval I = b(i);
		for (int j = 1; j < i; ++j) {
			if (!(A(i, j).isZero() || x(j).isZero())) {
				I -= A(i, j) * x(j);
			}
		}

		for (int j = i + 1; j <= A.nRows(); ++j) {
			if (!(A(i, j).isZero() || x(j).isZero())) {
				I -= A(i, j) * x(j);
			}
		}

		// TO BE REFINED USING THE EXTENDED INTERVAL DIVISION
		if (x(i).intersect(I / A(i, i)).isEmpty()) {
			return false;
		}
	}
	return true;
}


void NewtonReduction::Multiply(DenseMatrix<Interval> & m,
                               DenseMatrix<double> & p,
                               DenseMatrix<Interval> & a,
                               DenseMatrix<Interval> & zero)
{
	// m := p * a, the dimensions are supposed to be ok

	// Initialization of m to the zero matrix
	m = zero;

	for (int i = 1; i <= m.nRows(); ++i) {
		for (int j = 1; j <= m.nColumns(); ++j) {
			for (int k = 1; k <= p.nColumns(); ++k) {
				if (!(a(k, j).isZero() || (p(i, k) == 0.0))) {
					m(i, j) += Interval(p(i, k)) * a(k, j);
				}
			}
		}
	}
}


bool NewtonReduction::IsDiagonallyDominant(DenseMatrix<Interval> & A)
{
	Interval::Bound mag;
	int b;
	for (int i = 1; i <= A.nRows(); ++i) {
		if (i == 1) {
			mag = A(i, 2).mag();
			b = 3;
		} else {
			mag = A(i, 1).mag();
			b = 2;
		}
		Interval magsum(mag, mag);
		for (int j = b; j <= A.nRows(); ++j) {
			if (j != i) {
				mag = A(i, j).mag();
				magsum += Interval(mag);
			}
		}
		if (A(i, i).mig() < magsum.sup()) {
			return false;
		}
	}
	return true;
}


bool NewtonReduction::reductionAlgorithm()
{
	// Reduction wrt. the system F(X)=0 such that X in D
	// Creation of the linear system Jy = -F(m(D)) such that Y = X - m(X)

#ifdef ELISA_PROFILE
	NewtonReduction::ClockNewton.start();
	++ NewtonReduction::NumCall;
#endif


	// Creation of vectors mid = m(D) and b = -F(m(D))
	DenseMatrix<Interval> mid(_dim, 1), b(_dim, 1);
	this->evalMidpoint(mid, b);


	// Creation of the Jacobian matrix J
	DenseMatrix<Interval> J(_dim, _dim);
	this->evalJacobian(J);


	// Creation of vector y = D - m(D)
	DenseMatrix<Interval> y(_dim, 1);
	this->evalVariable(y, mid);


	// -------------------------------------------------
	// Preconditionning EVEN IF J is diagonally dominant
	// -------------------------------------------------
	// midJacobian := double matrix of midpoints of the Jacobian
	DenseMatrix<double> midJacobian(_dim, _dim);
	this->evalMidJacobian(midJacobian, J);

	// precond is the preconditionner
	DenseMatrix<double> precond(_dim, _dim);
	if (NewtonReduction::Inversion(precond, midJacobian, _MRid)) {
		// J := precond*J
		DenseMatrix<Interval> J2(_dim, _dim, Interval(0.0));
		NewtonReduction::Multiply(J2, precond, J, _MIzero);
		J = J2;

		// b := precond*b
		DenseMatrix<Interval> b2(_dim, 1);
		NewtonReduction::Multiply(b2, precond, b, _VIzero);
		b = b2;
	}
	// -------------------------------------------------


	// Gauss-Seidel iteration
	bool result = NewtonReduction::GaussSeidel(J, y, b);

	if (result) {
		result = this->reduceDomains(y, mid);
	}

#ifdef ELISA_PROFILE
	NewtonReduction::ClockNewton.stop();
#endif

	return result;
}


